//Sensors node firmware
//Mattia Marenco
//matiapascal@gmail.com
//Modified 27/04/2013 by Daniele Di Paola 

#include <OneWire.h>
#include <DallasTemperature.h>
#include <Wire.h>
#include <avr/power.h>

#define PI 3.14159265358979f
#define aMax 30.0f //maximum permitted inclination to the vertical
#define TMax +50.0f  //maximum permitted case temperature
#define TMin -10.0f //minimum permitted case temperature
#define NUM_DATI 14
#define T_CONV  188    //time needed for serial temperature sensors conversion 
#define ONE_WIRE_BUS 22  //PIN used as BUS

int sens_value[6] = {0,0,0,0,0,0};  // array of sensor data, accelerometers input initialization
int temperature_value[4] = {0,0,0,0};  // array of sensor data, temperatures input initialization
int temperature_value_prec[4] = {0,0,0,0}; // temperatures previous values temporary input array, initialization
int sens_offset[6] = {0,0,0,0,0,0}; // array of offset values for sensors
int gyro4x[3] = {0,0,0};  // array of Gyro amplified (4x) values - relative data 
int accel[3] = {0,0,0};  // array of accelerometer data - relative data 
float coef_ang[3]; //array angular coefficients accelerometers interpolated function
float zero_lev[3]; //array offset accelerometers interpolated function
float coef_ang_temp[4]; //array angular coefficients temperatures interpolated function
float zero_lev_temp[4]; //array offset temperatures interpolated function
float proiezione[3]; //array projections on the axes of gravity resultant
float risultante; //gravity resultant module, 1 on the earth
float angolo[3]; //resultant angles
float temperatura[4]; //temperatures
int allarme[13] ={0,0,0,0,0,0,0,0,0,0,0,0,1};// tilting alarm: on the left, right, forward, backward, temperature alarms low and high X4, initialization
byte i2c=0;
int Tc=1000; //ms sample rate >500ms
int temperatures[6]={0,0,0,0,0,0};;  //temporary array for one wire sensors

//all sensors in the lab
//DeviceAddress indirizzi [10]= { {0 0x28, 0xB7, 0xf5, 0xD, 0x4, 0x00, 0x00, 0xEB }, 
//                                {1 0x28, 0x2F, 0xCA, 0xD, 0x4, 0x00, 0x00, 0x10 },
//                                {2 0x28, 0xF3, 0xE4, 0xD, 0x4, 0x00, 0x00, 0xF3 },
//                                {3 0x28, 0xDE, 0xB9, 0xD, 0x4, 0x00, 0x00, 0x76 },
//                                {4 0x28, 0xB2, 0xBC, 0xD, 0x4, 0x00, 0x00, 0x1A },
//                                {5 0x28, 0x26, 0xD5, 0xD, 0x4, 0x00, 0x00, 0x95 },
//                                {6 0x28, 0xBF, 0xC8, 0xD, 0x4, 0x00, 0x00, 0x22 },
//                                {7 0x28, 0xBC, 0xE9, 0xD, 0x4, 0x00, 0x00, 0x4E },
//                                {8 0x28, 0x67, 0xEC, 0xD, 0x4, 0x00, 0x00, 0xA0 },
//                                {9 0x28, 0x15, 0xED, 0xD, 0x4, 0x00, 0x00, 0x9B }
//                              };
                              
//one wire sensor's address (fixed)                             
DeviceAddress indirizzi [6] =   {  { 0x28, 0x15, 0xED, 0xD, 0x4, 0x00, 0x00, 0x9B },
                                   { 0x28, 0xDE, 0xB9, 0xD, 0x4, 0x00, 0x00, 0x76 },
                                   { 0x28, 0xBC, 0xE9, 0xD, 0x4, 0x00, 0x00, 0x4E },
                                   { 0x28, 0xB7, 0xf5, 0xD, 0x4, 0x00, 0x00, 0xEB },
                                   { 0x28, 0x2F, 0xCA, 0xD, 0x4, 0x00, 0x00, 0x10 },
                                   { 0x28, 0xF3, 0xE4, 0xD, 0x4, 0x00, 0x00, 0xF3 }
                               };
                               
unsigned long int lastTimeRequest;
unsigned long int time_c;
	int ct;

OneWire oneWire(ONE_WIRE_BUS);    //set up sensor BUS on ONE_WIRE_BUS
DallasTemperature sensors(&oneWire);





void setup()

{
  sensors.begin();
  sensors.setWaitForConversion(false);  //async mode on

  //*********address initialization*************
  //TODO (important): put ALL this code in function like AddressInitialization
  //TODO: dynamic allocation (how?) to avoid all this stuff  
  //ALTERNATIVE: EVERY beginning of the program, recall function to get sensor's address

  //sensor 1 thermo1 = { 0x28, 0xB7, 0xf5, 0xD, 0x4, 0x00, 0x00, 0xEB };
  //indirizzi[0]=
  //indirizzi[0][0]=0x28; indirizzi[0][1]=0xB7; indirizzi[0][2]=0xf5; indirizzi[0][3]=0xD;
  //indirizzi[0][4]=0x4;  indirizzi[0][5]=0x00; indirizzi[0][6]=0x00; indirizzi[0][7]=0xEB;

  //sensor 2 thermo2 = { 0x28, 0x2F, 0xCA, 0xD, 0x4, 0x00, 0x00, 0x10 }
  //indirizzi[1][0]=0x28; indirizzi[1][1]=0x2F; indirizzi[1][2]=0xCA; indirizzi[1][3]=0xD; 
  //indirizzi[1][4]=0x4;  indirizzi[1][5]=0x00; indirizzi[1][6]=0x00; indirizzi[1][7]=0x10;

  //sensor 3 thermo3 = { 0x28, 0xF3, 0xE4, 0xD, 0x4, 0x00, 0x00, 0xF3 };
  //indirizzi[2][0]=0x28; indirizzi[2][1]=0xF3; indirizzi[2][2]=0xE4; indirizzi[2][3]=0xD; 
  //indirizzi[2][4]=0x4; indirizzi[2][5]=0x00; indirizzi[2][6]=0x00; indirizzi[2][7]=0xF3;

  //sensor 4 thermo4 = { 0x28, 0xDE, 0xB9, 0xD, 0x4, 0x00, 0x00, 0x76 };
  //indirizzi[3][0]=0x28; indirizzi[3][1]=0xDE; indirizzi[3][2]=0xB9; indirizzi[3][3]=0xD; 
  //indirizzi[3][4]=0x4; indirizzi[3][5]=0x00; indirizzi[3][6]=0x00; indirizzi[3][7]=0x76;

  //sensor 5 thermo5 = { 0x28, 0xB2, 0xBC, 0xD, 0x4, 0x00, 0x00, 0x1A };
  //indirizzi[4][0]=0x28; indirizzi[4][1]=0xB2; indirizzi[4][2]=0xBC; indirizzi[4][3]=0xD;
  //indirizzi[4][4]=0x4;  indirizzi[4][5]=0x00; indirizzi[4][6]=0x00; indirizzi[4][7]=0x1A;

  //sensor 6 thermo6 = { 0x28, 0x26, 0xD5, 0xD, 0x4, 0x00, 0x00, 0x95 };
  //indirizzi[5][0]=0x28; indirizzi[5][1]=0x26; indirizzi[5][2]=0xD5; indirizzi[5][3]=0xD; 
  //indirizzi[5][4]=0x4;  indirizzi[5][5]=0x00; indirizzi[5][6]=0x00; indirizzi[5][7]=0x95;

  //sensor 7 thermo7 = { 0x28, 0xBF, 0xC8, 0xD, 0x4, 0x00, 0x00, 0x22 };
  //indirizzi[6][0]=0x28; indirizzi[6][1]=0xBF; indirizzi[6][2]=0xC8; indirizzi[6][3]=0xD; 
  //indirizzi[6][4]=0x4;  indirizzi[6][5]=0x00; indirizzi[6][6]=0x00; indirizzi[6][7]=0x22;

  //sensor 8 thermo8 = { 0x28, 0xBC, 0xE9, 0xD, 0x4, 0x00, 0x00, 0x4E };
  //indirizzi[7][0]=0x28; indirizzi[7][1]=0xBC; indirizzi[7][2]=0xE9; indirizzi[7][3]=0xD;
  //indirizzi[7][4]=0x4;  indirizzi[7][5]=0x00; indirizzi[7][6]=0x00; indirizzi[7][7]=0x4E; 

  //sensor 9 thermo9 = { 0x28, 0x67, 0xEC, 0xD, 0x4, 0x00, 0x00, 0xA0 };
  //indirizzi[8][0]=0x28; indirizzi[8][1]=0x67; indirizzi[8][2]=0xEC; indirizzi[8][3]=0xD;
  //indirizzi[8][4]=0x4;  indirizzi[8][5]=0x00; indirizzi[8][6]=0x00; indirizzi[8][7]=0xA0;

  //sensor 10 thermo10 = { 0x28, 0x15, 0xED, 0xD, 0x4, 0x00, 0x00, 0x9B };
  //indirizzi[9][0]=0x28; indirizzi[9][1]=0x15; indirizzi[9][2]=0xED; indirizzi[9][3]=0xD;
  //indirizzi[9][4]=0x4;  indirizzi[9][5]=0x00; indirizzi[9][6]=0x00; indirizzi[9][7]=0x9B;

  //set sensor's resolution
  for(ct=0; ct<6; ct++)
    sensors.setResolution(indirizzi[ct], 10);

  //request temperatures for the first time
  sensors.requestTemperatures();
  lastTimeRequest = millis();
  time_c = millis();

  analogReference(EXTERNAL);     //using external analog ref of 3.3V for ADC scaling
  pinMode(13, OUTPUT); //led notifica 
  //Serial.begin(115200);  //setup serial baud rate
  Wire.begin(5);
  Wire.onRequest(requestEvent); 
  Wire.onReceive(receiveEvent);
  
  DDRC = B00000000;              // make all analog ports as inputs
  coef_ang[0] = 1.0f/103.0f;     //x angular coefficient                                 ULTIMARE CON CASE DEFINITIVO!!!
  zero_lev[0] = 507.0f/103.0f;   //x offset
  coef_ang[1] = 1.0f/105.0f;     //y angular coefficient 
  zero_lev[1] = 101.0f/21.0f;    //y offset
  coef_ang[2] = 1.0f/98.0f;      //z angular coefficient
  zero_lev[2] = 89.0f/14.0f;     //z offset
  coef_ang_temp[0] = 250.0f/3.0f;  //final coefficients obtained with thermic chamber, do not modify unless in case of hardware change and new calibration
  zero_lev_temp[0] = -98.13f;
  coef_ang_temp[1] = 250.0f/3.0f;
  zero_lev_temp[1] = -97.89f; 
  coef_ang_temp[2] = 250.0f/3.0f;
  zero_lev_temp[2] = -98.08f; 
  coef_ang_temp[3] = 250.0f/3.0f;
  zero_lev_temp[3] = -97.63f;  


  //Serial.print("Plase wait 60 seconds in order to stabilize sensors ");
  digitalWrite(13, HIGH);   // blink LED
  for (int a=0; a<60; a++)      // acquire data a lot of times in order to produce 
  {                             // good offset value - "warm-up" ADC
   delay(10);
   read_ADC();                  //read ADC sensor values
   readTemperData();            //read temperature sensors
   //Serial.print(".");           //print dot to show that Arduino is doing something
  }                                                     
  digitalWrite(13, LOW);



  for (int c=3; c<6; c++)       //calculate offset only on acceleration, remove the first three in future
  {
    sens_offset[c] = sens_value[c];       // store offset values in offset array
  } 
  sens_offset[2] = sens_offset[2] - 101; //adding -101 (raw value of one g) to z accelerometer 


  allarme[12]=0;                                

  //Serial.println("Done"); //Done calculating gyro and accelerometer offsets
  //Serial.println("---------");
  ct=0;
}

void loop()
{
  if( ( millis()-time_c ) > Tc)
  {
    digitalWrite(13, HIGH);

    read_ADC();            //read values from accelerometer sensors
    readTemperData();      //read values from temperature sensors


    //assign data to gyro and accelerometer arrays, with offset removal
    gyro4x[0] = delta_ADC(3); //gyro x 
    gyro4x[1] = delta_ADC(4); //gyro y 
    gyro4x[2] = delta_ADC(5); //gyro z 
    accel[0] = -delta_ADC(0);  //accel x 
    accel[1] = -delta_ADC(1);  //accel y 
    accel[2] = -delta_ADC(2);  //accel z 

    for (int axis=0; axis<3; axis++)
    {
      proiezione[axis]=getProiezione(axis); // get gravity projection on the three axis
    }

    risultante = getRisultante(); // get gravity resultant module
    for (int asse=0; asse<3; asse++)
    {
      angolo[asse]=getAngolo(asse); // get resultant angles, otherwise rover angles
      //Serial.print(angolo[asse]);
      //Serial.print(" , "); 
    }

    //Serial.print("  |  ");
    //Serial.print(risultante);
    //Serial.print("  |  ");

    for (int lato=0; lato<4; lato++)
    {
      temperatura[lato] = getTemperatura(lato);  // get case temperatures
      //Serial.print(temperatura[lato]);
      //Serial.print(" , ");
    }

    //Serial.print("  |  ");

    getAllarmeAngolo();         // get all the alarm (temp. and tilt)
    getAllarmeTemp();
    //for (int ai=0; ai<12; ai++) // print alarm
    //{
    //  Serial.print(allarme[ai]);
    //}

    //Serial.println("");

    //temperature acquisition

    if( (millis() - lastTimeRequest) > T_CONV )   //enter iff is passed enough time since the request of temperatures (time for conversion)
    { 
      //TODO: implementare tutto in una sola funzione
      //power_twi_disable();  //deactivate TWI
      TWCR &= ~(1<<2); // Disable I2C Engine
      if (ct==6)
         ct=0;
         
      temperatures[ct] = int(sensors.getTempC(indirizzi[ct]));
      //Serial.println(temperatures[ct]);
      ct++;
      sensors.requestTemperatures();
      lastTimeRequest = millis();
      //power_twi_enable();  //reactivate TWI
      TWCR |= (1<<2);
    }
    
  digitalWrite(13, LOW);
  time_c = millis();
  }

}    //end of loop

void read_ADC() //read ADC accellerometer sensor data
{
  for (int i=0; i<6; i++)
 { 
  sens_value[i] = ((analogRead(i) * 0.9)+ (sens_value[i] * 0.1)) ; // read the input pin and reduce noise with weighted media
 }
}

int delta_ADC(int index) //subtract offset from current value
{
  return (sens_value[index] - sens_offset[index]);
}

float getProiezione(int asse)    //transform the ADC result to real physical variables
{    
  static float tmpf;	        //temporary variable
  tmpf = accel[asse]*coef_ang[asse]+zero_lev[asse];  
  return tmpf;		
}

float getRisultante() //from euclidean sum gets the value of resultant
{
  float risultante;
  risultante = sqrt(squared(proiezione[0])+squared(proiezione[1])+squared(proiezione[2]));
  return risultante;
}

float getAngolo(int asse) // get resultant angles, otherwise rover angles 
{
  float angolo;
  angolo=acos(proiezione[asse]/risultante);
  angolo=180*angolo/PI;
  return angolo;
}

void getAllarmeAngolo()
{
  for (int i=0;i<4;i++)
    allarme[i]=0;

  if (angolo[2]<(180-aMax))
    {
    if (angolo[1]>(90+(aMax*0.2))) //0,2 margins of rounding
      allarme[0]=1;

    if (angolo[1]<(90-(aMax*0.2)))
      allarme[1]=1;

    if (angolo[0]>(90+(aMax*0.2)))
      allarme[2]=1;

    if (angolo[0]<(90-(aMax*0.2)))
      allarme[3]=1;
    }

  return;
}

float squared(float x){ //squared calculus
  return x*x;
}

void readTemperData() //read temperature data, noise reduction with weighted media
{
  int menouno; //previous data temporary
  for (int i=0; i<4; i++)
 { 
  menouno=temperature_value[i]; //store actual temperature to the -2t temporary variable
  temperature_value[i] = ((analogRead(12+i)*0.2) + (temperature_value[i] * 0.4) + (temperature_value_prec[i] * 0.4)); // read the input pin and reduce noise with weighted media
  temperature_value_prec[i]=menouno; //store the -2t temperature
 }
}

float getTemperatura(int lato)    //get C° temperatures from AD data
{    
  static float tmpf;	        //temporary variable
  tmpf = temperature_value[lato]*(3.3f/1024.0f)*coef_ang_temp[lato]+zero_lev_temp[lato];  // obtained from the equation of a line through two points multiplied by the conversion ratio of the ADC
  return tmpf;		
}

void getAllarmeTemp()            //out of range temperture alarm
{
  int h=0;    //counter for the conversion from index k to the input bit index
  for (int k=4;k<11;k=k+2)
    {
    allarme[k]=0;    //reset allarms
    allarme[k+1]=0;
    if (temperatura[k-4-h]<TMin) //get alarms
      {
        allarme[k]=1;
      }
    if (temperatura[k-4-h]>TMax)
      {
        allarme[k+1]=1;
      }
    h++;
    }
  return;
}

void receiveEvent(int num)
{
  i2c=Wire.read();

  if (i2c==10)
    Tc=1*1000;
  else if (i2c==11)
    Tc=2*1000;
  else if (i2c==12)
    Tc=5*1000;
  else if (i2c==13)
    Tc=10*1000;
  else if (i2c==14)
    Tc=30*1000;
  else if (i2c==15)
    Tc=60*1000;
}

void requestEvent()
{
  int dati[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
  byte i;
  for (i=0;i<4;i++)
  {
     dati[i]=int(temperatura[i]*10);
  }
  for (i=4;i<7;i++)
  {
     dati[i]=int(angolo[i-4]*10);
  }

  if (allarme[0]==1)
    bitSet(dati[7],0);
  if (allarme[1]==1)
    bitSet(dati[7],1);
  if (allarme[2]==1)
    bitSet(dati[7],2);
  if (allarme[3]==1)
    bitSet(dati[7],3);
  if (allarme[4]==1)
    bitSet(dati[7],4);
  if (allarme[5]==1)
    bitSet(dati[7],5);
  if (allarme[6]==1)
    bitSet(dati[7],6);
  if (allarme[7]==1)
    bitSet(dati[7],7);
  if (allarme[8]==1)
    bitSet(dati[7],8);
  if (allarme[9]==1)
    bitSet(dati[7],9);
  if (allarme[10]==1)
    bitSet(dati[7],10);
  if (allarme[11]==1)
    bitSet(dati[7],11);
  if (allarme[12]==1)
    bitSet(dati[7],12);

  if ((temperatures[3]>=50)||(temperatures[4]>=50)||(temperatures[5]>=50))  //convertitori
    bitSet(dati[7],13);
  if ((temperatures[0]>=40)||(temperatures[1]>=40))    //seriali 
    bitSet(dati[7],14);
  if (temperatures[2]>=60)    //pc
    bitSet(dati[7],15);
  

  dati[8]=temperatures[0]; //seriale1
  dati[9]=temperatures[1];  //seriale2
  dati[10]=temperatures[2];  //PC
  dati[11]=temperatures[3];  //conv1
  dati[12]=temperatures[4];  //conv2
  dati[13]=temperatures[5];  //conv3


  invia_i2c(dati,NUM_DATI);
}



void integerToBytes(int val, byte b[2]) {
  b[0] = (byte )((val >> 8) & 0xff);
  b[1] = (byte )(val & 0xff);
}

void invia_i2c (int *dato, byte num_dati)
{
  byte i=0;
  byte one[num_dati*2];
  for (i=0;i<num_dati; i++)
  {
    integerToBytes(dato[i], &one[i*2]); 
  }
   Wire.write(one,num_dati*2);
}
